/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
///GridClient.cpp
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include "gimi.h"
#include "TGIMI_PROTOCOL_GRID.h"
#include "TOccupancyGrid.h"
//#include "glutImageGUI.h"
#include "CGLHeightMap.h"
#include "glutHeightMapDisplay.h"


std::string paramHubAdress="asrobo.hut.fi";
int paramHubPort=50002;
std::string paramOwnName="grid_client";
std::string paramServer="grid_server";
int paramDebugLevel = 10;

void  printUsage(){
	fprintf(stderr,"Grid Client\n");
	fprintf(stderr,"Options:\n");
	fprintf(stderr,"-o string : set the own hub name\n");
	fprintf(stderr,"-d int : Set debug level\n");
	fprintf(stderr,"-p int : Set hub port\n");
	fprintf(stderr,"-s string : grid server name\n");
	fprintf(stderr,"-h string : Hub name\n");
	fprintf(stderr,"-? Usage\n");
	fprintf(stderr,"---------------------------------------\n");
	
}

void parse(int argc,char **argv){
	bool paramParseLoop = true;
	int option_char;
	/*** Set options ***/
	while (paramParseLoop) {
		option_char = getopt(argc, argv, "o:d:h:p:s:");
		switch (option_char) {
			case 'o':
				if (optarg != NULL)
					paramOwnName = optarg;
				fprintf(stderr,"Setting own name to %s\n",paramOwnName.c_str());
				break;
			case 'd':
				if (optarg != NULL)
					paramDebugLevel = atoi (optarg);
				fprintf(stderr,"Debug level=%d\n",paramDebugLevel);
				break;
			case 'h':
				if (optarg != NULL)
					paramHubAdress = optarg;
			  
				fprintf(stderr,"Hub adress set to %s\n",paramHubAdress.c_str());
				break;
			case 'p':
				if (optarg != NULL)
					paramHubPort = atoi (optarg);
				fprintf(stderr,"Hub port set to %d\n",paramHubPort);
				break;
			case 's':
				if (optarg != NULL)
					paramServer = optarg;
				fprintf(stderr,"Subscribing to %s\n",paramServer.c_str());
				break;
			case '?':
				printUsage();
				exit(1);
				break;
			case -1:
				paramParseLoop = false;
				break;
		}
	}
}

int main(int argc,char **argv){
		TGIMI_PROTOCOL_GRID_HEADER header;
		TGIMI_PROTOCOL_GRID netGrid;
		int gim_id=GIMI_PROTOCOL_GRID;				
		gimi::GIMI gim;		///< initialize with default queue size		
		parse(argc,argv);				
						
		/// Initialize the debugging module
		debugInit();
		debugSetGlobalDebugLvl(paramDebugLevel);
		debugSetMode(DEBUG_MODE_TIMESTAMP_HH_MM_SS_MS);
		
		gim.addWantedTypeId(gim_id);
		if(!gim.connectToHub(paramHubAdress.c_str(), paramHubPort, paramOwnName.c_str())){
				fprintf(stderr,"Connection to Hub '%s:%d' failed with name'%s'\n",
								paramHubAdress.c_str(),paramHubPort,paramOwnName.c_str());
				exit(1);
		}
		
		int retval = gim.subscribeToService(paramServer,gim_id,0,20000,20000);
		if(retval != GIMI_OK){
				fprintf(stderr,"Failed to subscribe to sevice=%d from '%s'\n",gim_id,paramServer.c_str());
				exit(1);
		}
		
		fprintf(stderr,"Client ready!\n");
		gimi::GIMIMessage msg; ///<GIMI message
		
		//TOccupancyGrid *O=NULL;
		//char *rgb;
		//float *gridptr; 
		
		CGLHeightMap *glhm = NULL;
		float *dataptr;
		
		
		while(1){
				retval = gim.receive(msg,20000);
				if(retval != GIMI_OK){
						if(retval == GIMI_TIMEOUT) fprintf(stderr,"No messages within 20000ms\n");
						else if(retval == GIMI_ERROR) fprintf(stderr,"GIMI ERROR!\n");
				}
				else{
						
						
						//fprintf(stderr,"Got message!\n");
						netGrid.handleNetPacket((unsigned char *) msg.data, msg.datalength);
						header = netGrid.getHeader();
						
						fprintf(stderr,"Size=%d (%dx%d) reso=%.2f\n",
										header.size,header.Nx,header.Ny,header.resolution);
						
						if(glhm == NULL){
								glhm = new CGLHeightMap(header.Nx,header.Ny,header.resolution);
								glut_hm_initialize(argc,argv,glhm);
						}
						
						glhm->setData((float *) netGrid.grid);
						
						
						/*********** USING GLUT IMAGE GUI ***********************
						if(O == NULL){
								O = new TOccupancyGrid(header.Nx*header.resolution,
																			 header.Ny*header.resolution,
																			 header.resolution,
																			 Grid::TCoord2D(0,0));
								rgb = (char *) malloc(3*header.Nx*header.Ny);
								addImageWindow(header.Nx, header.Ny);
								fprintf(stderr,"glutImageGUI_Start()\n");
								glutImageGUI_Start();
						}
						
						dataptr = (float *) netGrid.grid;
						gridptr = O->getDataPtr();
						for(int i = 0; i<header.Nx*header.Ny;i++){
								gridptr[i] = dataptr[i];
						}
						O->fillRGB((unsigned char *)rgb,1.0);		
						displayImage(0, (unsigned char *)rgb,header.Ny, header.Nx);
						*******************************************************************/
						
				}
		}				
		//delete O;	
		//free(rgb);			
						
	return 0;
}













